#include "mpu6050.h"
#include "main.h"
#include "myiic.h" 


_mpu6050_data mpu6050_data;
//初始化mpu6050触摸屏
//返回值:0,初始化成功;1,初始化失败 
esp_err_t mpu6050_init(void)
{
    esp_err_t ret =0;
  
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
    delay_ms(100);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050 
	delay_ms(100);
	MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
	MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
	MPU_Set_Rate(50);						//设置采样率50Hz
	MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效



	Init_MPU6050();

	ret=MPU_Read_Byte(MPU_DEVICE_ID_REG);
	if(ret==MPU_ADDR)//器件ID正确
	{
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
		MPU_Set_Rate(50);				//设置采样率为50Hz

		mpu6050_DEBUG("初始化成功:0x%x\r\n",ret);	

		mpu6050_data.temp = MPU_Get_Temperature();
		printf("温度:%d\r\n",mpu6050_data.temp);

		ret=0;

		xTaskCreate(mpu6050_test_task,"mpu6050_test_task",1024*2,NULL,20,NULL);
 	}else 
	{
		mpu6050_DEBUG("初始化失败:0x%x\r\n",ret);

		ret=1;
	}
	return ret;
}

void mpu6050_test_task(void *pvParameter)
{
	short gyrox,gyroy,gyroz;//陀螺仪原始数据
	while (1) 
    {
		MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //加速度&陀螺仪校准后的数据

		printf("%d %d %d\r\n",gyrox,gyroy,gyroz);
		mpu6050_data.temp = MPU_Get_Temperature();
		printf("温度:%d\r\n",mpu6050_data.temp);

		//READ_MPU6050_Accel();


        vTaskDelay(2000 / portTICK_PERIOD_MS); 
 	}
	vTaskDelete(NULL);
}


//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_LPF(u16 lpf)
{
	u8 data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器  
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Rate(u16 rate)
{
	u8 data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
 	return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2]; 
    short raw;
	float temp;
	MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((u16)buf[0]<<8)|buf[1];  
    temp=36.53+((double)raw)/340;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    u8 buf[6],res;  
	res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(res==0)
	{
		*gx=((u16)buf[0]<<8)|buf[1];  
		*gy=((u16)buf[2]<<8)|buf[3];  
		*gz=((u16)buf[4]<<8)|buf[5];
	} 	
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    u8 buf[6],res;  
	res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(res==0)
	{
		*ax=((u16)buf[0]<<8)|buf[1];  
		*ay=((u16)buf[2]<<8)|buf[3];  
		*az=((u16)buf[4]<<8)|buf[5];
	} 	
    return res;;
}
//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
	u8 i; 
    IIC_Start(); 
	IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令	
	if(IIC_Wait_Ack())	//等待应答
	{
		IIC_Stop();		 
		return 1;		
	}
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答
	for(i=0;i<len;i++)
	{
		IIC_Send_Byte(buf[i]);	//发送数据
		if(IIC_Wait_Ack())		//等待ACK
		{
			IIC_Stop();	 
			return 1;		 
		}		
	}    
    IIC_Stop();	 
	return 0;	
} 
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{ 
 	IIC_Start(); 
	IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令	
	if(IIC_Wait_Ack())	//等待应答
	{
		IIC_Stop();		 
		return 1;		
	}
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答
    IIC_Start();
	IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令	
    IIC_Wait_Ack();		//等待应答 
	while(len)
	{
		if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK 
		else *buf=IIC_Read_Byte(1);		//读数据,发送ACK  
		len--;
		buf++; 
	}    
    IIC_Stop();	//产生一个停止条件 
	return 0;	
}
//IIC写一个字节 
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data) 				 
{ 
    IIC_Start(); 
	IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令	
	if(IIC_Wait_Ack())	//等待应答
	{
		IIC_Stop();		 
		return 1;		
	}
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答 
	IIC_Send_Byte(data);//发送数据
	if(IIC_Wait_Ack())	//等待ACK
	{
		IIC_Stop();	 
		return 1;		 
	}		 
    IIC_Stop();	 
	return 0;
}
//IIC读一个字节 
//reg:寄存器地址 
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{
	u8 res;
    IIC_Start(); 
	IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令	
	IIC_Wait_Ack();		//等待应答 
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答
    IIC_Start();
	IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令	
    IIC_Wait_Ack();		//等待应答 
	res=IIC_Read_Byte(0);//读取数据,发送nACK 
    IIC_Stop();			//产生一个停止条件 
	return res;		
}





/* MPU6050 Register Address ------------------------------------------------------------*/
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG     0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)

#define ACCEL_XOUT_H 0x3B  //存储最近的XYZ轴加速度的值 
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40

#define TEMP_OUT_H 0x41  //存储最近温度值
#define TEMP_OUT_L 0x42

#define GYRO_XOUT_H 0x43  //存储最近XYZ轴的陀螺仪值
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48

#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据（接地，接电源为D2） 

unsigned char BUF[10];       //接收数据缓存区
char  test=0;   //IIC用到
short T_X,T_Y,T_Z,T_T;  //X,Y,Z轴，温度
//单字节写入*******************************************
bool Single_Write(unsigned char SlaveAddress1,unsigned char REG_Address,unsigned char REG_data)      //void
{
   IIC_Start(); 
    IIC_Send_Byte(SlaveAddress1);   //发送设备地址+写信号//I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);//设置高起始地址+器件地址 
    IIC_Wait_Ack();
    IIC_Send_Byte(REG_Address);   //设置低起始地址      
    IIC_Wait_Ack();
    IIC_Send_Byte(REG_data);
    IIC_Wait_Ack();   
    IIC_Stop(); 
    return true;
}
//单字节读取*****************************************
u8 Single_Read(unsigned char SlaveAddress2,unsigned char REG_Address)
{   
    u8 res;
  IIC_Start();  
    IIC_Send_Byte(SlaveAddress2); //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//设置高起始地址+器件地址 
    IIC_Wait_Ack();
    IIC_Send_Byte(REG_Address);   //设置低起始地址      
    IIC_Wait_Ack();
    IIC_Start();
    IIC_Send_Byte((SlaveAddress2+1));
    IIC_Wait_Ack();
  //res=IIC_Read_Byte(1);      //读高字节,发送ACK   
   //res<<=8;
   res = IIC_Read_Byte(0);      //读低字节,发送NACK    
   IIC_Stop(); //产生一个停止条件 
return res; //返回读到的数据
}
/********初始化MPU6050**************/
void Init_MPU6050(void)
{
  Single_Write(SlaveAddress,PWR_MGMT_1,0x00);     //解除休眠状态
delay_ms(500);
Single_Write(SlaveAddress,SMPLRT_DIV,0x07);      //陀螺仪采样
//Single_Write(SlaveAddress,107,0x03);
Single_Write(SlaveAddress,CONFIG, 0x06);          //5Hz 
Single_Write(SlaveAddress,GYRO_CONFIG,0x18);     //采样率
Single_Write(SlaveAddress,ACCEL_CONFIG,0x01);    //滤波频率
}
/*******************从MPU6050中读取数据**************************/
void read_mpu6050(void)
{
   BUF[0]=Single_Read(SlaveAddress,GYRO_XOUT_L); 
   BUF[1]=Single_Read(SlaveAddress,GYRO_XOUT_H);
   T_X= (BUF[1]<<8)|BUF[0];
   T_X/=16.4;     //读取计算X轴数据
   BUF[2]=Single_Read(SlaveAddress,GYRO_YOUT_L);
   BUF[3]=Single_Read(SlaveAddress,GYRO_YOUT_H);
   T_Y= (BUF[3]<<8)|BUF[2];
   T_Y/=16.4;     //读取计算Y轴数据
   BUF[4]=Single_Read(SlaveAddress,GYRO_ZOUT_L);
   BUF[5]=Single_Read(SlaveAddress,GYRO_ZOUT_H);
   T_Z= (BUF[5]<<8)|BUF[4];
   T_Z/=16.4;         //读取计算Z轴数据
}

void READ_MPU6050_Accel(void)
{
	BUF[0]=Single_Read(SlaveAddress,ACCEL_XOUT_L); 
	BUF[1]=Single_Read(SlaveAddress,ACCEL_XOUT_H);
	T_X= (BUF[1]<<8)|BUF[0];
	T_X=(float)((float)T_X/16384)*100;  //扩大100倍        //读取计算X轴数据

	BUF[2]=Single_Read(SlaveAddress,ACCEL_YOUT_L);
	BUF[3]=Single_Read(SlaveAddress,ACCEL_YOUT_H);
	T_Y= (BUF[3]<<8)|BUF[2];
	T_Y=(float)((float)T_Y/16384)*100;     //读取计算Y轴数据
	
	BUF[4]=Single_Read(SlaveAddress,ACCEL_ZOUT_L);
	BUF[5]=Single_Read(SlaveAddress,ACCEL_ZOUT_H);
	T_Z= (BUF[5]<<8)|BUF[4];
	T_Z=(float)((float)T_Z/16384)*100;         //读取计算Z轴数据

	BUF[6]=Single_Read(SlaveAddress,TEMP_OUT_L); 
	BUF[7]=Single_Read(SlaveAddress,TEMP_OUT_H); 
	T_T=(BUF[7]<<8)|BUF[6];
	// T_T = 35+ ((double) (T_T + 13200)) / 280;// 读取计算出温度
	T_T = (((double) T_T )/340+36.53)*10 ;//+36.53;  // 读取计算出温度

	printf("温度:%d\r\n",T_T);

}





